#ifndef __lidar209_RAWDATA_H
#define __lidar209_RAWDATA_H

#include <errno.h>
#include <stdint.h>
#include <string>
#include <boost/format.hpp>
#include <math.h>

#include <rclcpp/rclcpp.hpp>
#include <pcl/point_cloud.h>
#include <sensor_msgs/msg/point_cloud2.hpp>
#include <sensor_msgs/point_cloud2_iterator.hpp>


#include <lidar209_msgs/msg/lidar209_scan.hpp>
#include <lidar209_msgs/msg/lidar209_scan_ch72.hpp>
#include <lidar209_msgs/msg/lidar209_scan_ch88.hpp>

#include "datacontainerbase.hpp"


#include "point_types.h"
#include "calibration.h"

namespace lidar209_rawdata
{
// Shorthand typedefs for point cloud representations
typedef lidar209_pointcloud::PointXYZIR VPoint;
typedef pcl::PointCloud<VPoint> VPointCloud;
typedef unsigned char  byte;
/**
   * Raw lidar209 packet constants and structures.
   */


static const int Num_Of_Detector=64;
static const int Num_Of_Detector_CH72=72;
static const int Num_Of_Detector_CH88=88;
static const int Num_Of_Detector_CH176=176;

/**每包中为24组，每组16（18）×3+3 */
static const int RAW_SCAN_SIZE = 3;
static const int SIZE_BLOCK = 51; 
static const int SIZE_BLOCK_CH72 = 57;
static const int SIZE_BLOCK_CH88 = 69;
static const int SCANS_PER_BLOCK = 16;
static const int SCANS_PER_BLOCK_CH72 = 18;
static const int SCANS_PER_BLOCK_CH88 = 22;
static const int SCANS_PER_BLOCK_CH176 = 44;

static const int BLOCK_DATA_SIZE = (SCANS_PER_BLOCK * RAW_SCAN_SIZE);
static const int BLOCK_DATA_SIZE_CH72 = (SCANS_PER_BLOCK_CH72 * RAW_SCAN_SIZE);
static const int BLOCK_DATA_SIZE_CH88 = (SCANS_PER_BLOCK_CH88 * RAW_SCAN_SIZE);

static const float ROTATION_RESOLUTION      =     0.01f;  // [deg]
static const uint16_t ROTATION_MAX_UNITS    = 36000;     // [deg/100]
static const float DISTANCE_RESOLUTION      =     0.0075f; // [m]

union two_bytes
{
  uint16_t uint;
  unsigned char   bytes[2];
};



/** \brief Raw lidar209 data block.
   *
   *  Each block contains data from either the upper or lower laser
   *  bank.  The device returns three times as many upper bank blocks.
   *
   *  use stdint.h types, so things work with both 64 and 32-bit machines
   *  每包中为24组，每组16（18）×3+3
   */

typedef struct raw_block
{
  unsigned char  header;        ///< UPPER_BANK or LOWER_BANK
  unsigned char  rotation[2];      ///< 0-35999, divide by 100 to get degrees
  unsigned char   data[BLOCK_DATA_SIZE];
} raw_block_t;

typedef struct raw_block_CH72
{
  unsigned char  header;        ///< UPPER_BANK or LOWER_BANK
  unsigned char  rotation[2];      ///< 0-35999, divide by 100 to get degrees
  unsigned char   data[BLOCK_DATA_SIZE_CH72];
} raw_block_t_CH72;

typedef struct raw_block_CH88
{
  unsigned char  header;        ///< UPPER_BANK or LOWER_BANK
  unsigned char  rotation[2];      ///< 0-35999, divide by 100 to get degrees
  unsigned char   data[BLOCK_DATA_SIZE_CH88];
} raw_block_t_CH88;


/** used for unpacking the first two data bytes in a block
   *
   *  They are packed into the actual data stream misaligned.  I doubt
   *  this works on big endian machines.
   */
static const int PACKET_SIZE = 1232;
static const int PACKET_SIZE_CH72 = 1376;
static const int PACKET_SIZE_CH88 = 1449;
static const int BLOCKS_PER_PACKET = 24;
static const int BLOCKS_PER_PACKET_CH72 = 24;
static const int BLOCKS_PER_PACKET_CH88 = 20;
//static const int PACKET_STATUS_SIZE = 4;
static const int SCANS_PER_PACKET = (SCANS_PER_BLOCK * BLOCKS_PER_PACKET);
static const int SCANS_PER_PACKET_CH72 = (SCANS_PER_BLOCK_CH72 * BLOCKS_PER_PACKET);
static const int SCANS_PER_PACKET_CH88 = (SCANS_PER_BLOCK_CH88 * BLOCKS_PER_PACKET_CH88);
/** \brief Raw lidar209 packet.
   *
   *  revolution is described in the device manual as incrementing
   *    (mod 65536) for each physical turn of the device.  Our device
   *    seems to alternate between two different values every third
   *    packet.  One value increases, the other decreases.
   *
   *  \todo figure out if revolution is only present for one of the
   *  two types of status fields
   *
   *  status has either a temperature encoding or the microcode level
   */

typedef struct raw_packet
{
  raw_block_t blocks[BLOCKS_PER_PACKET];
  unsigned char  revolution[4];
  unsigned char  type;
  unsigned char  status2[3];
} raw_packet_t;

typedef struct raw_packet_CH72
{
  raw_block_t_CH72 blocks[BLOCKS_PER_PACKET];
  uint32_t  revolution;
  uint32_t unused;
  uint32_t  sn;
  uint32_t  tmp_detect;
  uint32_t  tmp_fpga;
  uint32_t  v_high;
  uint32_t  rpm;
  unsigned char  ver[6];
  unsigned char  return_num[4];
  uint16_t  th1;
  uint16_t  th2;
  uint16_t  th3;
  uint32_t  frq;
  uint16_t mode1;
  uint16_t mode2;
  unsigned char  unused2[5];
} raw_packet_t_CH72;

typedef struct raw_packet_CH88
{
  raw_block_t_CH88 blocks[BLOCKS_PER_PACKET_CH88];
  uint32_t  revolution;//时间戳
  //unsigned char    unused1;
  unsigned char status[65];
  /*uint32_t    sn;

   uint32_t    tmp_detect;
  int32_t  tmp_detect;
 uint32_t  tmp_fpga;
  uint32_t  v_high;
  uint32_t  rpm;
  unsigned char  ver[6];
 unsigned char  return_num[4];
  uint16_t  th1;
  uint16_t  th2;
uint16_t  th3;
  uint16_t  frq;
  uint16_t  saturation;//切换饱和与非饱和
  uint16_t mode1;//单双脉冲模式
  uint16_t mode2;
   uint16_t v_high_detect;//探测器高压截距
   uint16_t tmp_v_high_detect;//探测器高压温度系数
  uint8_t corection;//校正与否
   uint32_t tmp_laser;//激光器温度
  uint16_t UDP_port;//UDP端口号
  unsigned char  unused2[10];*/
} raw_packet_t_CH88;

/** \brief lidar209 data conversion class */
class RawData final
{
public:

  explicit RawData(rclcpp::Node * private_nh);
  ~RawData() {}

  //void unpack_ch72(const lidar209_msgs::lidar209Packet_CH72 &pkt, VPointCloud &pc);
  void unpack_ch88(const lidar209_msgs::msg::Lidar209PacketCH88 &pkt, DataContainerBase &pc);
  void unpack_ch176(const lidar209_msgs::msg::Lidar209PacketCH88 &pkt, DataContainerBase &pc);
  void setParameters(double min_range, double max_range, double view_direction, double view_width);
  int setup();
  int scansPerPacket() const;
  int numLasers() const;

  unsigned int recieved_time;
  unsigned int sn;
  int tmp_detect;
  int tmp_fpga;
  unsigned int v_high;
  unsigned int rpm;
  unsigned int ver;
  unsigned int return_num;
  unsigned int th1;
  unsigned int th2;
  unsigned int th3;
  unsigned int frq;
  unsigned int mode1;
  unsigned int mode2;
  unsigned int saturation;//切换饱和与非饱和
  unsigned int v_high_detect;//探测器高压截距
  unsigned int tmp_v_high_detect;//探测器高压温度系数
  unsigned int correction;//校正与否
  unsigned int tmp_laser;//激光器温度
  unsigned int UDP_port;//UDP端口号
  std::string topicname;
  bool m_b_updown;
  bool m_b_laser_hz;
  double m_d_rpm;
  /** configuration parameters */
  typedef struct {
    std::string calibrationFile;     ///< calibration file name
    double max_range;                ///< maximum range to publish
    double min_range;                ///< minimum range to publish

    int min_angle;                   ///< minimum angle to publish
    int max_angle;                   ///< maximum angle to publish

    double tmp_min_angle;
    double tmp_max_angle;

    int show_id;
    int show_group;
    int Rotation_Angle;
    int Beta_Angle;
    int Alpha_Angle;
    int ARR_Angle1;
    int ARR_Angle2;
    int ARR_Angle3;
    int ARR_Angle4;
    std::string model;
  } Config;
  Config config_;

private:
  rclcpp::Node * private_nh_;
  /**
     * Calibration file
     */
  lidar209_pointcloud::Calibration calibration_;
  double sin_rot_table_[ROTATION_MAX_UNITS];
  double cos_rot_table_[ROTATION_MAX_UNITS];

  int alpha[Num_Of_Detector];
  int thita[Num_Of_Detector];
  float cos_thita_value[Num_Of_Detector];
  float sin_thita_value[Num_Of_Detector];

  float R_corr[Num_Of_Detector];
  float Z_corr[Num_Of_Detector];



  int alpha_CH72[Num_Of_Detector_CH72];
  int thita_CH72[Num_Of_Detector_CH72];
  float cos_thita_value_CH72[Num_Of_Detector_CH72];
  float sin_thita_value_CH72[Num_Of_Detector_CH72];

  float R_corr_CH72[Num_Of_Detector_CH72];
  float Z_corr_CH72[Num_Of_Detector_CH72];



  int alpha_CH88[Num_Of_Detector_CH88];
  int thita_CH88[Num_Of_Detector_CH88];
  float cos_thita_value_CH88[Num_Of_Detector_CH88];
  float sin_thita_value_CH88[Num_Of_Detector_CH88];

  float R_corr_CH88[Num_Of_Detector_CH88];
  float Z_corr_CH88[Num_Of_Detector_CH88];

  double dis_pre_CH88[Num_Of_Detector_CH88];
  double dis_pre_CH176[Num_Of_Detector_CH176];

  double laser_angle[18];//72
  double dis_error[18];//72

  double laser_angle_CH88[SCANS_PER_BLOCK_CH176];
  double dis_error_CH88[SCANS_PER_BLOCK_CH88];


  double dis_5678[2]={0,0};
  int mirror_alpha[4];
  int mirror_alpha_error[4];
  float fire_Serial_Time[4][16];
  float fire_Serial_Time_CH72[4][18];
  float fire_Serial_Time_CH72_2[18];

  float fire_Serial_Time_CH88[24];//本应该为SCANS_PER_BLOCK_CH88（22），考虑编码的方便，改为24，即4组，每组6个。
  float fire_Serial_Time_CH176[44];

  float alpha_ontime;//同一组数据中每个点之间的水平角度偏差
  float dis_tmp;
  int rho_ang;


  int bytes2ToInt(unsigned char *b);
  int bytesToInt(unsigned char b);
  /** in-line test whether a point is in range */
  bool pointInRange(float range)
  {
    return (range >= config_.min_range
            && range <= config_.max_range);
  }

  void InitTables_chaowu_CH88();
  void InitTables_chaowu_CH176();
  void InitTables_zw_CH176();

  void  CalculateXYZ_CH88(double range, int angle, int laser_ID,VPoint &pp,double &deg_x,double &deg_y);
  void  CalculateXYZ_CH176(double range, int angle, int laser_ID, VPoint &pp, double &DegX, double &DegY,int mode);

  double cal_cos(int angle);
  double cal_sin(int angle);
  int calib_angle(int angle);
  bool interval_shielding(int current_angle,unsigned int angle_view);

};

} // namespace lidar209_rawdata

#endif // __lidar209_RAWDATA_H
